"""Implements classes for data collection in Carla."""

# %%
# The following boilerplate is required if .egg is not installed
# See: https://carla.readthedocs.io/en/latest/build_system/
import glob
import os
import sys
import re
import random
import queue
from collections import deque
import time
import numpy as np
from types import SimpleNamespace
# from carlasim.groundtruth import GroundTruthExtractor
# from carlasim.carla_tform import Transform
# from agents.navigation.behavior_agent import BehaviorAgent

# try:
#     sys.path.append(glob.glob('./carla-*%d.%d-%s.egg' % (
#         sys.version_info.major,
#         sys.version_info.minor,
#         'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
# except IndexError:
#     pass
import carla

# %% ================= Global function =================


# %% ================= Geo2Location =================

# TODO: Replace this with pyproj?


class Geo2Location(object):
    """
    Helper class for homogeneous transform from geolocation

    This class is used by GNSS class to transform from carla.GeoLocation to carla.Location.
    This transform is not provided by Carla, but it can be solved using 4 chosen points.
    Note that carla.Location is in the left-handed coordinate system.
    """

    def __init__(self):
        """ Constructor method """
        
        self._tform =  np.array([[0.000000000000000000e+00,1.113194907932735805e+05,0.000000000000000000e+00,0.000000000000000000e+00]
                                        ,[1.113194908268951112e+05,-0.000000000000000000e+00,-0.000000000000000000e+00,-0.000000000000000000e+00]
                                        ,[0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00,0.000000000000000000e+00]
                                        ,[0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00]])
    
        # self._tform = np.linalg.inv(g).dot(l)

    def transform_gnss2carla(self, geolocation):
        """
        Transform from carla.GeoLocation to carla.Location (left_handed z-up).

        Numerical error may exist. Experiments show error is about under 1 cm in Town03.
        """
        geoloc = np.array(
            [geolocation.latitude, geolocation.longitude, geolocation.altitude, 1])
        carla_loc = self._tform.dot(geoloc.T)
        return carla_loc
    
    def transform_carla2gnss(self, carla_location):
        """
        Transform from carla.GeoLocation to carla.Location (left_handed z-up).

        Numerical error may exist. Experiments show error is about under 1 cm in Town03.
        """
        carlaloc = np.array(
            [carla_location.x, carla_location.y, carla_location.z, 1])
        invM = np.linalg.inv(self._tform)
        G=invM.dot(carlaloc)
        return G

    def get_matrix(self):
        """ Get the 4-by-4 transform matrix """
        return self._tform

    def set_matrix(self,matrix):
        """ set the 4-by-4 transform matrix """
        self._tform = matrix

# %% ================= Sensor Base =================


def save_numpy(array,name):
    path_txt=f'./transferMatric_G2L/{name}.txt'
    path_np=f'./transferMatric_G2L/{name}.npy'
    np.savetxt(path_txt, array)
    np.save(path_np, array)

def load_numpy_txt(name):
    path_txt=f'./transferMatric_G2L/{name}.txt'
    # path_np=f'./transferMatric_G2L/{name}.npy'
    return np.loadtxt(path_txt)
    # np.load(path_np)

def load_numpy_np(name):
    # path_txt=f'./transferMatric_G2L/{name}.txt'
    path_np=f'./transferMatric_G2L/{name}.npy'
    # np.load(path_txt)
    return np.load(path_np)

if __name__ == '__main__':
    # carla_loc -> geo_loc
    gObj=Geo2Location()
    carla_location = SimpleNamespace()
    carla_location.x=77.7
    carla_location.y=-107.3
    carla_location.z=0
    print(f">>>>>>假设carla坐标值:carla location x:{carla_location.x},y:{carla_location.y},z:{carla_location.z}")
    M=gObj.get_matrix()
    print(M)
    geolocation=gObj.transform_carla2gnss(carla_location)
    print(geolocation)

    # geo_loc -> carla_loc
    geolocation = SimpleNamespace()
    geolocation.longitude=0.000707
    geolocation.latitude=-0.000964
    geolocation.altitude=1
    carla_location = gObj.transform_gnss2carla(geolocation)
    print(f"carla_location:{carla_location}")


